# 四足机器人小跑步态控制模块
import PA_AVGFILT
import PA_IMU
import array

from math import sin,cos,atan,pi
from machine import I2C, Pin

import dog_cfg
import sys_control

#滑动平均滤波(PITCH 轴)
gyro_data_p = array.array('i', [0]*60)
f_gyro_data_p = PA_AVGFILT.avg_filiter(gyro_data_p)


def cal_t(t,xs,xf,raiseH,r1,r2,r3,r4):    #小跑步态执行函数
    """
    t: 时间参数，用于控制步态周期
    xs: 起始位置x坐标
    xf: 目标位置x坐标
    raiseH: 抬腿高度
    r1,r2,r3,r4: 方向控制参数，用于判断转向
    """
    Tm=dog_cfg.faai*dog_cfg.Ts    # 步态周期时间
    Hc=abs(dog_cfg.init_y)    # 髋关节高度
    z=0       # 转向角度
    S0=xf     # 目标位置x坐标

    # 获得陀螺仪数据
    ay = PA_IMU.acc.get_values()
    q = PA_IMU.IMUupdate(ay["GyX"], ay["GyY"], ay["GyZ"], ay["AcX"], ay["AcY"], ay["AcZ"])
    try:
      gyro_p=f_gyro_data_p.avg(round(q[0]))  # 获取滤波后的陀螺仪pitch角度
    except:
      pass
    # 方向判断
    if r1==-1 and r2==-1 and r3==1 and r4==1:   # 左转
        z=-10
    elif r1==1 and r2==1 and r3==-1 and r4==-1:   # 右转
        z=10
    

    H0=raiseH  # 抬腿高度
    a=0        # 初始角度
    d=atan(z/Hc)  # 转向角度计算 横向角度

    if t<Tm:  # 前半步态周期
        sigma=2*pi*t/Tm
        c=cos(d*cos(pi*t/Tm))  # 转向角度余弦值 角度越大补偿值越大 
        b=(1-cos(sigma))/2  # 抬腿高度系数
        x0=S0*(t/Tm-1/(2*pi)*sin(sigma))-0.5*S0  # x轴位移
        y0=(H0*b)/c  # y轴位移 腿横向跨度越大，高度就越低，除以补偿值c就是弥补高度
        z0=a+z*cos(pi*t/Tm)  # z轴位移
        x3=x0
        y3=y0
        z3=z0
        x1=-x0
        y1=(-H0/40*b)/c #支撑腿往下撑一点
        z1=z0
        x2=x1
        y2=y1
        z2=z0
    
    if t>=Tm:  # 后半步态周期
        sigma=2*pi*(t-Tm)/Tm
        c=cos(d*cos(pi*t/Tm))  # 转向角度余弦值
        b=(1-cos(sigma))/2  # 抬腿高度系数
        x0=0.5*S0-S0*((t-Tm)/Tm-1/(2*pi)*sin(sigma))  # x轴位
        y0=(-H0/40*b)/c  # y轴位移
        z0=a+z*cos(pi*t/Tm)  # z轴位移
        x3=x0
        y3=y0
        z3=z0
        x1=-x0
        y1=(H0*b)/c
        z1=z0
        x2=x1
        y2=y1
        z2=z0

    import dog_data
    
    # 斜坡面步态规划曲线转换
    if dog_cfg.Left!=0 and dog_cfg.Right!=0 and dog_cfg.key_stab==True:
        if gyro_p>0:
          sita=(-gyro_p*(dog_data.Kp_TW))*pi/180  # 计算倾斜角度
        elif gyro_p<0:
          sita=(-gyro_p*(dog_data.Kp_TW))*pi/180
        else:
          sita=0
        x_m_0=(y0+Hc)*sin(sita)+x0*cos(sita)  # 转换x坐标
        y_m_0=(y0+Hc)*cos(sita)-x0*sin(sita)  # 转换y坐标
        x0=x_m_0
        y0=y_m_0-Hc
    
        x_m_1=(y1+Hc)*sin(sita)+x1*cos(sita)
        y_m_1=(y1+Hc)*cos(sita)-x1*sin(sita)
        x1=x_m_1
        y1=y_m_1-Hc
    
        x_m_2=(y2+Hc)*sin(sita)+x2*cos(sita)
        y_m_2=(y2+Hc)*cos(sita)-x2*sin(sita)
        x2=x_m_2
        y2=y_m_2-Hc
    
        x_m_3=(y3+Hc)*sin(sita)+x3*cos(sita)
        y_m_3=(y3+Hc)*cos(sita)-x3*sin(sita)
        x3=x_m_3
        y3=y_m_3-Hc
    else:
      pass
    
    # 返回所有关节的x,y,z坐标
    return x0,x1,x3,x2,y0,y1,y3,y2,z0,z1,z3,z2






